﻿using System.Diagnostics;
using System.Linq;
using Microsoft.Dss.Core.Attributes;
using System;
using System.IO;
using System.Collections.Generic;
using System.Drawing;


namespace NestedParticleFilter
{
    /// <summary>
    /// Enetity Class, representing squared objects such as walls and landmarks
    /// </summary>
    public class Entity
    {

        protected String _name;  //name of the object read from a text file
        protected Point _center;  //Center of the landmark (x,z) locations in meters
        protected CustomisedColor _entityColor; //Customized color normally used for landmarks
        
        /// <summary>
        /// Default Constructor
        /// </summary>
        public Entity()
        {
            _entityColor = new CustomisedColor();
        }
        /// <summary>
        /// Overloaded constructor, Instantiate the entity object
        /// </summary>
        /// <param name="name">Name of the entity</param>
        /// <param name="center">center of the entity</param>
        /// <param name="entityColor">color of the entity</param>
        public Entity(String name, Point center, CustomisedColor entityColor)
        {
            this._name = name;
            this._center = center;
            this._entityColor = entityColor;
        }
        /// <summary>
        /// name of the entity
        /// </summary>
        public String Name
        {
            get { return _name; }
            set { _name = value; }
        }
        /// <summary>
        /// center of the entity
        /// </summary>
        public Point Center
        {
            get { return this._center; }
            set { this._center = value; }
        }
        /// <summary>
        /// entity's color
        /// </summary>
        public CustomisedColor EntityColor
        {
            get { return this._entityColor; }
            set { this._entityColor = value; }
        }

    }
    /// <summary>
    /// Landmark class
    /// </summary>
    public class Landmark : Entity, ICloneable
    {
        //reading landmark information off of the text file. The positions as well as the color.
        
        public const int factorH = 15;
        public const double factorS = 0.5;
        public const double factorL = 0.5;
        
        #region Data members
        bool _isDynamic;
        readonly double _length;
        readonly double _width;
        Point _p1; 
        Point _p2; 
        Point _p3; 
        Point _p4;
        Color _landmarkColor; // for walls. It is used to draw a colorful map on the screen.
        #endregion

        #region Constructors

        public object Clone()
        {
            Point center = new Point(this.Center.X, this.Center.Z);
            CustomisedColor color = new CustomisedColor(this._entityColor.Hue, this._entityColor.Saturation, this._entityColor.Luminance);
            String name = this.Name;
            double length = this._length;
            double width = this._width;
            Landmark copy = new Landmark(name, center, length, width, color);
            return copy;
        }

        public Landmark()
        {
        }
        /// <summary>
        /// Constructor with given landmark information
        /// </summary>
        /// <param name="name">name of the landmark</param>
        /// <param name="center">center of the landmark</param>
        /// <param name="length">length of the landmark</param>
        /// <param name="width">width of the landmark</param>
        public Landmark(String name, Point center, double length, double width)
        {
            _name = name;
            _center = center;
            _width = width;
            _length = length;

            double lowX = _center.X - length / 2;
            double highX = _center.X + length / 2;
            double lowZ = _center.Z - width / 2;
            double highZ = _center.Z + width / 2;

            _p1 = new Point(highX, highZ);
            _p2 = new Point(lowX, highZ);
            _p3 = new Point(highX, lowZ);
            _p4 = new Point(lowX, lowZ);
            _entityColor = new CustomisedColor();

        }
        /// <summary>
        /// Constructor with given landmark information used for walls with default color
        /// </summary>
        /// <param name="name">name of the landmark</param>
        /// <param name="center">center of the landmark</param>
        /// <param name="length">length of the landmark</param>
        /// <param name="width">width of the landmark</param>
        /// <param name="landmarkColor">color of the landmark</param>
        public Landmark(String name, Point center, double length, double width, Color landmarkColor)
            : this(name, center, length, width)
        {
            _landmarkColor = landmarkColor;
        }

        /// <summary>
        /// Constructor with given landmark information. Used for landmark objects with customized color
        /// </summary>
        /// <param name="name"></param>
        /// <param name="p"></param>
        /// <param name="length"></param>
        /// <param name="width"></param>
        /// <param name="color"></param>
        public Landmark(String name, Point p, double length, double width, CustomisedColor color)
            : this(name, p, length, width)
        {

            _entityColor.Hue = color.Hue;
            _entityColor.Saturation = color.Saturation;
            _entityColor.Luminance = color.Luminance;
        }
        #endregion

        #region Getters and Setters

        public double Length
        {
            get { return this._length; }
        }
        public double Width
        {
            get { return this._width; }
        }

        public bool IsDynamic
        {
            get { return this._isDynamic; }
            set { this._isDynamic = value; }
        }

        public Point P1
        {
            get { return this._p1; }
            set { this._p1 = value; }
        }
        public Point P2
        {
            get { return this._p2; }
            set { this._p2 = value; }
        }

        public Point P3
        {
            get { return this._p3; }
            set { this._p3 = value; }
        }
        public Point P4
        {
            get { return this._p4; }
            set { this._p4 = value; }
        }

        public Color LandmarkColor
        {
            get { return this._landmarkColor; }
            set { this._landmarkColor = value; }
        }

        public override String ToString()
        {
            return this._name + " (" + Center.X + ", " + Center.Z + ")";
        }

        /// <summary>
        /// When the object moves in the map this will recalculate the locations of the new object and 
        /// its geographical corner poses
        /// </summary>
        public void RecalculateCornerPoints()
        {
            double lowX = _center.X - _length / 2;
            double highX = _center.X + _length / 2;
            double lowZ = _center.Z - _width / 2;
            double highZ = _center.Z + _width / 2;

            this.P1.X = highX;
            this.P1.Z = highZ;
            this.P2.X = lowX;
            this.P2.Z = highZ;
            this.P3.X = highX;
            this.P3.Z = lowZ;
            this.P4.X = lowX;
            this.P4.Z = lowZ;

        }

        /// <summary>
        /// Calculates the z value of a point when rotated along the y axis based on the given angle
        /// </summary>
        /// <param name="angle">angle to rotate in radian</param>
        /// <param name="point">point of interest</param>
        /// <returns>new z value of the point</returns>
        private double RotateZ(double angle, Point point)
        {
            return (point.Z - _center.Z) * Math.Cos(angle) - (point.X - _center.X) * Math.Sin(angle) + _center.Z;
        }

        /// <summary>
        /// calculates the x value of a point when rotated along the y axis based on the given angle
        /// </summary>
        /// <param name="angle">angle to rotate</param>
        /// <param name="point">point of interest</param>
        /// <returns>new x value</returns>
        private double RotateX(double angle, Point point)
        {
            return (point.Z - _center.Z) * Math.Sin(angle) + (point.X - _center.X) * Math.Cos(angle) + _center.X;
        }
        /*******************************************************************************/
        /// <summary>
        /// Rotates the coordinates x and z based on the angle
        /// </summary>
        /// <param name="angle">angle in radian</param>
        /********************************************************************************/
        public void RotateCoordiantes(double angle)
        {
            double newX = RotateX(angle, this.P1);
            double newZ = RotateZ(angle, this.P1);
            this.P1.X = newX;
            this.P1.Z = newZ;
            newX = RotateX(angle, this.P2);
            newZ = RotateZ(angle, this.P2);
            this.P2.X = newX;
            this.P2.Z = newZ;
            newX = RotateX(angle, this.P3);
            newZ = RotateZ(angle, this.P3);
            this.P3.X = newX;
            this.P3.Z = newZ;
            newX = RotateX(angle, this.P4);
            newZ = RotateZ(angle, this.P4);
            this.P4.X = newX;
            this.P4.Z = newZ;
            //Console.WriteLine(this.toString());
        }

        #endregion


        #region Static Methods

        /******************************************************************************************************/
        
        /// <summary>
        /// This function reads the landmarks specification from the text file and saves them into a list for
        /// the robot's beliefs purposes
        /// </summary>
        /// <returns>List of landmarks object which includes the pose of the landmarks as well as 
        ///  their customized color information</returns>
        public static List<Landmark> ReadBelievedLandmarks(String fileName)
        {
            List<Landmark> lmList = null;
            //Create a reader and open the file
            TextReader textReader;
            try
            {

                textReader = new StreamReader(fileName); // read the file 
                String line;
                lmList = new List<Landmark>();
                do
                {
                    line = textReader.ReadLine(); //read a line of text
                    if (line == null)
                        break;
                    String[] array = line.Split(' '); // split the line by space
                    for (int i = 0; i < array.Length; )
                    {
                        String name = array[i++];

                        //setting the position of the landmark
                        double cx = double.Parse(array[i++]);   // x center of the landmark
                        double cz = double.Parse(array[i++]);   // z center of the landmark
                        Point center = new Point(cx, cz);
                        double length = double.Parse(array[i++]);
                        double width = double.Parse(array[i++]);
                        Landmark lm = new Landmark(name, center, length, width);

                        // setting the color of the landmark
                        double red = double.Parse(array[i++]);
                        lm._entityColor.Hue = red;
                        double green = double.Parse(array[i++]);
                        lm._entityColor.Saturation = green;
                        double blue = double.Parse(array[i++]);
                        lm._entityColor.Luminance = blue;

                        if (lm.Name.Contains("blue"))
                            lm.LandmarkColor = Color.Blue;
                        else if (lm.Name.Contains("pink"))
                            lm.LandmarkColor = Color.Magenta;
                        else if (lm.Name.Contains("green"))
                            lm.LandmarkColor = Color.LimeGreen;


                        String dynamic = array[i++];
                        if (dynamic.Equals("true"))
                            lm.IsDynamic = true;
                        else
                            lm.IsDynamic = false;
                        lmList.Add(lm);
                        //Console.WriteLine(lm.toString());
                    }
                }
                while (line != null);

                //close the stream
                textReader.Close();
            }
            catch (Exception e)
            {
                Console.WriteLine(e.ToString());
            }

            return lmList;
        }

        /// <summary>
        /// Identify list of Identified landmarks based on the color that is observed
        /// </summary>
        /// <param name="knownLandmarks">known landmarks to the user (read from txt file)</param>
        /// <param name="observedColor">Observed HSL color</param>
        /// <param name="depth">The depth in which the color has been identified as landmark color</param>
        /// <param name="angle">Angle in degree</param>
        /// <returns>Returns list of identified landmarks if any, null if no landmark is identified</returns>
        /// <exception cref="ArgumentNullException"><paramref name="knownLandmarks" /> is <c>null</c>.</exception>
        public static List<IdentifiedLandmark> IdentifyLandmarkByColor(List<Landmark> knownLandmarks, CustomisedColor observedColor, double depth, double angle)
        {
            if (knownLandmarks == null) throw new ArgumentNullException("knownLandmarks");
            if (observedColor == null) throw new ArgumentNullException("observedColor");
            
            List<IdentifiedLandmark> identifiedLandmarks = new List<IdentifiedLandmark>();
            foreach (var knownLandmark in knownLandmarks)
            {
                if (observedColor.Hue >= (knownLandmark.EntityColor.Hue - factorH)
                    && observedColor.Hue <= (knownLandmark.EntityColor.Hue + factorH)
                    && observedColor.Saturation >= (knownLandmark.EntityColor.Saturation - factorS)
                    && observedColor.Saturation <= (knownLandmark.EntityColor.Saturation + factorS)
                    && observedColor.Luminance >= (knownLandmark.EntityColor.Luminance - factorL)
                    && observedColor.Luminance <= (knownLandmark.EntityColor.Luminance + factorL))
                {
                    Landmark lm = (Landmark)knownLandmark.Clone();

                    identifiedLandmarks.Add(new IdentifiedLandmark(lm, depth, angle, observedColor));
                }
            }
           return identifiedLandmarks;
        }


        /// <summary>
        /// Find the landmark that is facing toward the robot among all the landmarks that are identified
        /// as observed. This is usefull because there might be many landmarks that are said identified and share
        /// same charactristics (like colors) . The landmark that is more likely to faceing the robot is the one that the particle should 
        /// be weight based on and not the one that is perhaps closer to the robot but the robot is not facing
        /// toward it, and hence not in the field of its camera. 
        /// </summary>
        /// <param name="landmarks">identified landmarks</param>
        /// <param name="particlePose">particle's pose</param>
        /// <returns>a landmark that faces toward the robot</returns>
        public static IdentifiedLandmark ClosestLandmarkByAngle(List<IdentifiedLandmark> landmarks, RobotPose particlePose)
        {
            double minOrientation = 360; // in degree
            double minDistance = 4000; // m
            const double observableDistance = 8;
            IdentifiedLandmark closestLandmark = null;
            int i = 0;
            int count = landmarks.Count;
           // Debug.WriteLine("\tParticle's Pose (degree)" + particlePose);
            do
            {
                // distance between the landmark and particle
                double distance = Math.Sqrt(
                    Math.Pow(landmarks[i].Landmark.Center.X - particlePose.X, 2) +
                    Math.Pow(landmarks[i].Landmark.Center.Z - particlePose.Z, 2));
                
                if (distance < observableDistance)
                {
                    if (distance < minDistance)
                        minDistance = distance;
                    //find the orientation of the particle to the landmark.
                    double orientation = Math.Atan2(-landmarks[i].Landmark._center.X + particlePose.X, -landmarks[i].Landmark._center.Z + particlePose.Z); // in radian
                    orientation -= UnitConversion.DegreeToRadian(particlePose.Theta);
                    orientation = UnitConversion.RadianToDegree(UnitConversion.PutThetaInBound(orientation)); // degree
                   // Debug.WriteLine("\torientation of the particle from " + landmarks[i].Landmark.Name + " is: " + orientation);
                    if (Math.Abs(orientation) <= minOrientation)
                    {
                        minOrientation = Math.Abs(orientation);
                        closestLandmark = landmarks[i];
                    }
                }
                else 
                {
                    if (distance < minDistance)
                    {
                        minDistance = distance;
                        closestLandmark = landmarks[i];
                    }
                }
                
                
                i++;
            } while (i < count);
            if (closestLandmark == null)
            {
                Debug.WriteLine("closest landmark cannot be null");
                Debug.WriteLine("Particle's Pose: " + particlePose.ToString());
                Debug.Assert(closestLandmark != null, "closest landmark Cannot be null");
            }
            return closestLandmark;
        }

        /// <summary>
        /// This function first filters those landmarks that are inthe field of view of the robot. Then among them it finds the closest landmark that
        /// is in between the upper and lower bound threshold. It ruturns the closest landmark to a robot's pose (particle's pose) that is in the field of view of that particle
        /// and the distance between the particle and the landmark is between the upper and lower bound.
        /// </summary>
        /// <param name="pose">hypothesised pose</param>
        /// <param name="landmarks">list of landmarks </param>
        /// <param name="fieldOfView">field of view in degree</param>
        /// <param name="upperbound">upper bound in meters</param>
        /// <param name="lowerbound">lower bound in meters</param>
        /// <returns>the closest landmark if any and null otherwise</returns>
        public  static Landmark ClosestLandmarkByThreshold (RobotPose pose, List<Landmark> landmarks, double fieldOfView, double lowerbound, double upperbound)
        {
           fieldOfView /= 2; // degree to the left/right of the robot
            List<Landmark> list = FindClosestLandmarksByFieldOfView(pose, landmarks, fieldOfView);
            if (list.Count != 0)
            {
                for (int i = 0; i < list.Count; i++)
                {
                    // calculate the distance of x from landmark
                    double distance = Math.Sqrt(
                        Math.Pow(list[i]._center.X - pose.X, 2) +
                        Math.Pow(list[i]._center.Z - pose.Z, 2)); // in meters
                    if (distance > lowerbound && distance < upperbound)
                        return list[i];            
                }   
            }
           return null;
        }
        
        /// <summary>
        /// This function will return list of landmarks that are in the field of view of the robot.
        /// </summary>
        /// <param name="pose">robot's pose</param>
        /// <param name="landmarks">list of landmarks </param>
        /// <param name="fieldOfView">field of view of the robot</param>
        /// <returns></returns>
        private static List<Landmark> FindClosestLandmarksByFieldOfView(RobotPose pose, List<Landmark> landmarks, double fieldOfView)
        {
            List<Landmark> list = new List<Landmark>();
            fieldOfView /= 2; // degree to the left/right of the robot
            for (int i = 0; i < landmarks.Count; i++)
            {
                // The atan2 gives the angle between two points and the positive x (in our siumlation will be z) axis.
                // Since we would like to masure this from the particle's position, the reference point (center point)
                // will be the position of the particle and hence it will be landmark x - particle x, and landamrk z -particle z
                // instead of the other way around.
                // now this will be giving the angle considering +z axis will point to 0 degree. however in our simulation +z axis
                // points to 180 degree. So we need to deduct 180 degree to adjust it.
                double theta = Math.Atan2(
                    landmarks[i]._center.X - pose.X,
                    landmarks[i]._center.Z - pose.Z);
                theta = UnitConversion.RadianToDegree(theta) -180;
                //Debug.WriteLine("Atan2 : " + theta);
                theta = pose.Theta - theta;
                theta = Math.Abs(theta);
                if ((theta > 0 && theta < fieldOfView) )
                    list.Add(landmarks[i]);
            }
            return list;
        }


         /// <summary>
        /// Finds the closest landmark to the robot based on the distance regardless of the robot's orientation
        /// </summary>
        /// <param name="landmarks">List of known landmarks</param>
        /// <param name="pose">Robot's pose</param>
        /// <returns>Closest Landmark</returns>
        public static Landmark ClosestLandmarkByDistance(List<Landmark> landmarks, RobotPose pose)
        {
             double minDistance = 1000;
            Landmark closestLandmark = null;
            int i = 0;
            int count = landmarks.Count;
            do
            {
                //find the orientation of the particle to the landmark.
                double distance = Math.Sqrt(Math.Pow(landmarks[i].Center.X - pose.X, 2) + Math.Pow(landmarks[i].Center.Z - pose.Z, 2));
                if (distance < minDistance)
                {
                    minDistance = distance;
                    closestLandmark = landmarks[i];
                }
                i++;
            } while (i < count);
            return closestLandmark;
        }

        /// <summary>
        /// Find the index of a landmark given a list
        /// </summary>
        /// <param name="list">list of landmarks</param>
        /// <param name="landmark">specific landmark</param>
        /// <returns></returns>
        public static int FindIndexOfLandmark(List<Landmark> list, Landmark landmark)
        {
            for (int i = 0; i < list.Count; i++)
            {
                if (list[i]._name.Equals(landmark._name))
                    return i;
            }
            return -1;
        }

        ///****************************************************************************************************************************************/
        ///// <summary>
        ///// 
        ///// </summary>
        ///// <param name="landmarks"></param>
        ///// <param name="p"></param>
        ///// <returns>distance in millimeters</returns>
        ///***************************************************************************************************************************************/
        //public static double distanceFromClosestLandmark(List<Landmark> landmarks, RobotPose p)
        //{
        //    Landmark closestLM = findClosestLandmarkByOrientation(landmarks, p);
        //    if (closestLM == null || p == null)
        //        return 0;
        //    double distance;
        //    distance = Math.Sqrt(Math.Pow(closestLM.Center.X - p.X, 2) + Math.Pow(closestLM.Center.Z - p.Z, 2));
        //    return distance * 1000;

        //}

        ///*********************************************************************************************************************************/
        ///// <summary>
        ///// Return the closest landmark in the range of the threshold to the pose x
        ///// </summary>
        ///// <param name="list"> List of landmarks</param>
        ///// <param name="x">Pose of the robot/particle</param>
        ///// <param name="threshold">threshold</param>
        ///// <param name="camFOV" > Camera Field of view (To the left OR to the right, not total) </param>
        ///// <returns></returns>
        ///**********************************************************************************************************************************/
        //public static Landmark findClosestLandmark(List<Landmark> list, RobotPose x, double threshold, double camFOV)
        //{
        //    for (int i = 0; i < list.Count; i++)
        //    {
        //        // calculate the distance of x from landmark
        //        double distance = Math.Sqrt(Math.Pow(list[i]._center.X - x.X, 2) + Math.Pow(list[i]._center.Z - x.Z, 2));
        //        double theta = Math.Atan2(-list[i]._center.X + x.X, -list[i]._center.Z + x.Z);
        //        theta -= MathHelper.DegreeToRadian(x.theta);
        //        theta = MathHelper.RadianToDegree(MathHelper.PutThetaInBound(theta)); // in degree
        //        theta = Math.Abs(theta);
        //        if ((theta > 0 && theta < camFOV) && distance < threshold)
        //            return list[i];
        //    }
        //    return null;
        //}

       
        #endregion

    }


    /// <summary>
    /// Identified landmark is a landmark that has been identified from a certain range and angle by the robot.
    /// The range is the kinect depth in which the landmark has been recoginzed by the robot
    /// The range unit is in mm.
    /// </summary>
    public class IdentifiedLandmark
    {
        public Landmark Landmark { set; get; } // landmark information
        public double Range { set; get; } // in mm
        public double Angle { set; get; } // in degree
        public CustomisedColor ObservedColor { set; get; }

        /// <summary>
        /// Identif
        /// </summary>
        /// <param name="landmark">Landmark information</param>
        /// <param name="range">the depth in which the landmark has been recoginzed</param>
        /// <param name="angle">angle in which the object was observed</param>
        /// <param name="observedColor">the color of the observed object</param>
        public IdentifiedLandmark(Landmark landmark, double range, double angle, CustomisedColor observedColor)
        {
            Landmark = landmark;
            Range = range;
            Angle = angle;
            ObservedColor = observedColor;
        }
        public override string ToString()
        {
            return Landmark.ToString() + " depth(mm): " + Range +" angle(degree) "+ Angle;
        }

    }

    /// <summary>
    /// Customized class for color in order to set the HSL values
    /// </summary>
    public class CustomisedColor
    {
        double _hue;
        double _saturation;
        double _luminance;

        public CustomisedColor()
        {
            _hue = 0;
            _saturation = 0;
            _luminance = 0;
        }

        public CustomisedColor(double hue, double saturation, double luminance)
        {
            this._hue = hue;
            this._saturation = saturation;
            this._luminance = luminance;
        }
        public double Hue
        {
            get { return this._hue; }
            set { this._hue = value; }
        }
        public double Saturation
        {
            get { return this._saturation; }
            set { this._saturation = value; }
        }

        public double Luminance
        {
            get { return this._luminance; }
            set { this._luminance = value; }
        }
        public override string ToString()
        {
            return _hue + " " + _saturation + " " + _luminance;
        }
    }
}
